
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_rangefinder.h
  * @author     baiyang
  * @date       2023-2-9
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include "sensor_rf_backend.h"
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
typedef struct sensor_rangefinder* sensor_rangefinder_t;
typedef const struct sensor_rangefinder* sensor_rangefinder_const_t;

struct sensor_rangefinder {
    struct sensor_rf_params params[RANGEFINDER_MAX_INSTANCES];
    struct RangeFinder_State state[RANGEFINDER_MAX_INSTANCES];

    sensor_rf_backend_t drivers[RANGEFINDER_MAX_INSTANCES];

    uint8_t num_instances;
    bool init_done;
    rt_mutex_t detect_sem;
    float estimated_terrain_height;
    Vector3f_t pos_offset_zero;      // allows returning position offsets of zero for invalid requests
    uint32_t _log_rfnd_bit;          // 
};
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/*
  initialise the RangeFinder class. We do detection of attached range
  finders here. For now we won't allow for hot-plugging of
  rangefinders.
 */
void sensor_rngfnd_init();

/*
  update RangeFinder state for all instances. This should be called at
  around 10Hz by main loop
 */
void sensor_rngfnd_update(void);

void sensor_rngfnd_set_log_rfnd_bit(uint32_t log_rfnd_bit);

/*
  Return the number of range finder instances. Note that if users
  sets up rangefinders with a gap in the types then this is the
  index of the maximum sensor ID plus one, so this gives the value
  that should be used when iterating over all sensors
*/
uint8_t sensor_rngfnd_num_sensors(void);

// get rangefinder type for an ID
enum RangeFinderType sensor_rngfnd_get_type(uint8_t id);

// get rangefinder address (for AP_Periph CAN)
uint8_t sensor_rngfnd_get_address(uint8_t id);

/*
  set an externally estimated terrain height. Used to enable power
  saving (where available) at high altitudes.
 */
void sensor_rngfnd_set_estimated_terrain_height(float height);

sensor_rf_backend_t sensor_rngfnd_get_backend(uint8_t id);
enum RangeFinderStatus sensor_rngfnd_status_orient(enum RotationEnum orientation);

// find first range finder instance with the specified orientation
sensor_rf_backend_t sensor_rngfnd_find_instance(enum RotationEnum orientation);

void sensor_rngfnd_handle_msg(const mavlink_message_t *msg);

// return true if we have a range finder with the specified orientation
bool sensor_rngfnd_has_orientation(enum RotationEnum orientation);

float sensor_rngfnd_distance_orient(enum RotationEnum orientation);
uint16_t sensor_rngfnd_distance_cm_orient(enum RotationEnum orientation);

int16_t sensor_rngfnd_max_distance_cm_orient(enum RotationEnum orientation);
int16_t sensor_rngfnd_min_distance_cm_orient(enum RotationEnum orientation);

int16_t sensor_rngfnd_ground_clearance_cm_orient(enum RotationEnum orientation);
bool sensor_rngfnd_has_data_orient(enum RotationEnum orientation);
uint8_t sensor_rngfnd_range_valid_count_orient(enum RotationEnum orientation);
const Vector3f_t* sensor_rngfnd_get_pos_offset_orient(enum RotationEnum orientation);
uint32_t sensor_rngfnd_last_reading_ms(enum RotationEnum orientation);
MAV_DISTANCE_SENSOR sensor_rngfnd_get_mav_distance_sensor_type_orient(enum RotationEnum orientation);
bool sensor_rngfnd_get_temp(enum RotationEnum orientation, float *temp);
bool sensor_rngfnd_prearm_healthy(char *failure_msg, const uint8_t failure_msg_len);

// Write an RFND (rangefinder) packet
void sensor_rngfnd_log_rfnd();
/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



